// This is a update on a magmouse opensource project for git. 
// Reference: https://github.com/sb-ocr/diy-spacemouse/tree/main/Code

#include <Tlv493d.h>
#include <SimpleKalmanFilter.h>
#include <TinyUSB_Mouse_and_Keyboard.h>
#include <OneButton.h>


Tlv493d mag = Tlv493d();
SimpleKalmanFilter xFilter(1, 1, 0.7), yFilter(1, 1, 0.7), zFilter(1, 1, 0.7);  // Modify the parameter to make it convergence more quickly

// Setup buttons
OneButton button1(0, true);   // Change button pin according to your hardware connection
OneButton button2(1, true);   // Change button pin according to your hardware connection

float xOffset = 0, yOffset = 0, zOffset = 0;
float xCurrent = 0, yCurrent = 0, zCurrent = 0;

int calSamples = 30;    // Change this samples from 300 to 30, to shorter the time on initialization.
int sensivity = 8;
int magRange = 3;
int outRange = 50;      // Max allowed in HID report, Change this range according to your preference.
float xyThreshold = 0.4; // Center threshold, Change this parameter according to your observation on the output of the serial.

int inRange = magRange * sensivity;
float zThreshold = xyThreshold / 1;   // In my case, the zThreshould is the same, this is based on the observation of serial output.

bool isOrbit = false;
#define LED_PIN 6        // Change this according to your hardware connection.

int fadeAmount=3;
int brightness=15;

void setup()
{
  pinMode(LED_PIN, OUTPUT);
  analogWrite(LED_PIN, 100);
  // mouse and keyboard init
  Mouse.begin();              // Mouse and keyboard should begin before the wire initializatiopn.
  Keyboard.begin();

  Serial.begin(115200);       // Prepare the serial.
  while (!Serial);
  
  // Serial.println(F("\nSend any character to begin DMP programming and demo: "));
  // while (Serial.available() && Serial.read()); // empty buffer
  // while (!Serial.available());                 // wait for data
  // while (Serial.available() && Serial.read()); // empty buffer again
  Serial.println("Setup Started...");

  button1.attachClick(goHome);
  button1.attachLongPressStop(goHome);

  button2.attachClick(fitToScreen);
  button2.attachLongPressStop(fitToScreen);

  Serial.println("Button Setup ok");
  


  Serial.println("Mouse and Keyboard Setup ok");

  
  //Wire.setSDA(4u);    // Change this value if your I2C is connected to other PIN
  //Wire.setSCL(5u);    // Change this value if your I2C in connected to other PIN
  Wire.begin();

  Serial.println("Wire Setup ok");



  // mag sensor init
  mag.begin(Wire);
  Serial.println("Mag Setup ok");
  

  mag.setAccessMode(mag.MASTERCONTROLLEDMODE);  
  mag.disableTemp();


  // crude offset calibration on first boot
  for (int i = 1; i <= calSamples; i++)
  {

    // delay(mag.getMeasurementDelay());
    delay(50);
    mag.updateData();

    xOffset += mag.getX();
    yOffset += mag.getY();
    zOffset += mag.getZ();

    Serial.print(".");
  }

  xOffset = xOffset / calSamples;
  yOffset = yOffset / calSamples;
  zOffset = zOffset / calSamples;

  Serial.println();
  Serial.println(xOffset);
  Serial.println(yOffset);
  Serial.println(zOffset);


}

void loop()
{

  // keep watching the push buttons
  button1.tick();
  button2.tick();

  // get the mag data
  // delay(mag.getMeasurementDelay());
  delay(20);
  mag.updateData();

  // update the filters
  xCurrent = xFilter.updateEstimate(mag.getX() - xOffset);
  yCurrent = yFilter.updateEstimate(mag.getY() - yOffset);
  zCurrent = zFilter.updateEstimate(mag.getZ() - zOffset);

  // check the center threshold
  if (abs(xCurrent) > xyThreshold || abs(yCurrent) > xyThreshold)
  {

    int xMove = 0;
    int yMove = 0;
    float xMoveTmp, yMoveTmp;

    // map the magnetometer xy to the allowed 127 range in HID repports
    xMove = map(xCurrent*0.9/xyThreshold, -inRange, inRange, -outRange, outRange);    // change from xcurrent to xcurrent*0.9/xyThreshold to make it more fluently
    yMove = map(yCurrent*0.9/xyThreshold, -inRange, inRange, -outRange, outRange);    // change from ycurrent to ycurrent*0.9/xyThreshold to make it more fluently


    // press shift to orbit in Fusion 360 if the pan threshold is not corssed (zAxis)
    if (abs(zCurrent) < zThreshold && !isOrbit)
    {
      Keyboard.press(KEY_LEFT_SHIFT);
      isOrbit = true;
    }

    // pan or orbit by holding the middle mouse button and moving propotionaly to the xy axis
    Mouse.press(MOUSE_MIDDLE);
    Mouse.move(-xMove, -yMove, 0);    // This somewhat depends on the placement of the sensor.
    Serial.print("Mouse Move: ");
    Serial.print(-xMove);
    Serial.print(", ");
    Serial.println(-yMove);
  }
  else
  {

    // release the mouse and keyboard if within the center threshold
    Mouse.release(MOUSE_MIDDLE);
    Keyboard.releaseAll();
    isOrbit = false;
  }

  Serial.print(xCurrent);
  Serial.print(",");
  Serial.print(yCurrent);
  Serial.print(",");
  Serial.print(zCurrent);
  Serial.println();

  analogWrite(LED_PIN, brightness);       // LED
  brightness = brightness + fadeAmount;

  // reverse the direction of the fading at the ends of the fade:


  if (brightness <= 3 || brightness >= 250) {
    fadeAmount = -fadeAmount;
  }
}

// go to home view in Fusion 360 by pressing  (CMD + SHIFT + H) shortcut assigned to the custom Add-in command
void goHome()
{
  Keyboard.press(KEY_LEFT_GUI);
  Keyboard.press(KEY_LEFT_SHIFT);
  Keyboard.write('h');

  delay(10);
  Keyboard.releaseAll();
  Serial.println("pressed home");
}

// fit to view by pressing the middle mouse button twice
void fitToScreen()
{
  Mouse.press(MOUSE_MIDDLE);
  Mouse.release(MOUSE_MIDDLE);
  Mouse.press(MOUSE_MIDDLE);
  Mouse.release(MOUSE_MIDDLE);

  Serial.println("pressed fit");
}
